﻿#include "io.h"

IO::IO( QObject* parent ) : QObject( parent ) {
// UPDTE 2023.03.13 初始化IO获取出需要定义的io模块数目和模块的地址

#ifdef INPUT_TWO
    io_type_ = QBitArray( 6, false );  //设置io模块的总io数（包括输入和输出），4*16=64位  64/32=2个模块
    io_type_.setBit( 4 );              //如果是do则设为1,di则设为0
    io_type_.setBit( 5 );              // io_type_[0]=0; io_type_[1]=0;io_type_[2]=0;  io_type_[3]=0; io_type_[4]=1; io_type_[5]=1;
#endif
#ifdef INPUT_ONE
    io_type_ = QBitArray( 4, false );  //设置io模块的总io数（包括输入和输出），4*16=64位  64/32=2个模块
    io_type_.setBit( 2 );              //如果是do则设为1,di则设为0
    io_type_.setBit( 3 );              // io_type_[0]=0; io_type_[1]=0;io_type_[2]=0;  io_type_[3]=0; io_type_[4]=1; io_type_[5]=1;
#endif
}
// UPDATE 2023.03.11 从程序输入io的key获取iodefineNew.ini对应的value
int IO::getDiKeytoValue( IO::DiDefine diIndex ) {
    QString key = QMetaEnum::fromType< IO::DiDefine >().valueToKey( diIndex );
    foreach ( Param::ioParam demo, Param::input_io_hash_table ) {
        if ( key == demo.key ) { return demo.id; }
    }
    sigSendTips( "错误，程序输入io的key:" + key + "无法从文件找到对应！请检查iodefineNew.ini" );
    emit sigIoDefineStrError();
    return 0;
}
/**
 * getDiKeytoValue和getDoKeytoValue 是程序根据enum名称去获取的时候再找对应表
 * 而下面的toggleDO是直接根据屏幕的按钮去触发对应的点位，所以它不需要判断名称
 * */
int IO::getDoKeytoValue( IO::DoDefine doIndex ) {
    QString key = QMetaEnum::fromType< IO::DoDefine >().valueToKey( doIndex );
    foreach ( Param::ioParam demo, Param::out_io_hash_table ) {
        if ( key == demo.key ) { return demo.id; }
    }
    sigSendTips( "错误，程序输出io的key:" + key + "无法从文件找到对应！请检查iodefineNew.ini" );
    emit sigIoDefineStrError();
    return 0;
}
IO::IOState IO::getDi( IO::DiDefine diIndex ) {
    unsigned int di32;
    int          keyid = getDiKeytoValue( diIndex );
    if ( keyid < 32 )  //第一个输入模块范围[0,31]
        di32 = io_32in;
    else  //第二个输入模块[32,64]
    {
        di32 = io_32in_2;
        keyid -= 32;  //移位
    }

    return ( ( ( di32 >> getDiKeytoValue( diIndex ) ) & 0x01 ) == 0x01 ) ? IO::SET : IO::RESET;
}

IO::IOState IO::getDo( IO::DoDefine doIndex ) {
    unsigned int do32;
    do32 = io_32out;
    return ( ( ( do32 >> getDoKeytoValue( doIndex ) ) & 0x01 ) == 0x01 ) ? IO::SET : IO::RESET;
}

void IO::setDo( IO::DoDefine doIndex, IO::IOState value ) {
    unsigned int do32;

    do32          = io_32out;
    int doIndex_2 = getDoKeytoValue( doIndex );
    if ( value == IO::SET ) {
        do32 |= ( 1 << doIndex_2 );  //设置对应位数的值为1  do32=do32|(1 << doIndex)
    }
    else if ( value == IO::RESET ) {
        do32 &= ~( 1 << doIndex_2 );  //清除对应位数的值
    }
    else if ( value == IO::TOGGLE ) {
        if ( ( ( do32 >> doIndex_2 ) & 0x01 ) == 0x01 ) {
            do32 &= ~( 1 << doIndex_2 );  //清除对应位数的值
        }
        else {
            do32 |= ( 1 << doIndex_2 );  //设置对应位数的值为1
        }
    }
    io_32out = do32;
}

void IO::toggleDO( int id ) {
    unsigned int do32;
    do32 = io_32out;

    if ( ( ( do32 >> id ) & 0x01 ) == 0x01 ) {
        do32 &= ~( 1 << id );  //清除对应位数的值
    }
    else {
        do32 |= ( 1 << id );  //设置对应位数的值为1
    }
    io_32out = do32;
}

void IO::resetAllDo() {
    io_32out = 0x00000000;
    ioControlSetIoState();
    is_allow_setting = false;
}

void IO::breakHandler() {
    io_32out = 0x00000000;
    setDo( LEFT_SERVO_DRIVER_ENABLE, RESET );
    setDo( RIGHT_SERVO_DRIVER_ENABLE, RESET );
    setDo( RED_LED, SET );
    setDo( RED_LED, SET );
    setDo( WORKING_MUSIC_4, SET );
    ioControlSetIoState();
    is_allow_setting = false;
}

void IO::readIoState() {
    //初始化IO
    int              num = io_type_.size() * 16;
    static QBitArray io_state( num );
    unsigned int     value = 0;
    int              mode  = 16;
    int              k;

    value = io_32in;
    k     = 0;
    for ( int i = mode * k; i < mode * ( k + 1 ); i++ ) {
        io_state.setBit( i, ( ( ( value >> ( i ) ) & 0x01 ) == 0x01 ) );
    }
    k++;
    for ( int i = mode * k; i < mode * ( k + 1 ); i++ ) {
        io_state.setBit( i, ( ( ( value >> ( i ) ) & 0x01 ) == 0x01 ) );
    }
#ifdef INPUT_TWO
    value = io_32in_2;
    k++;
    for ( int i = mode * k; i < mode * ( k + 1 ); i++ ) {
        io_state.setBit( i, ( ( ( value >> ( i ) ) & 0x01 ) == 0x01 ) );
    }
    k++;
    for ( int i = mode * k; i < mode * ( k + 1 ); i++ ) {
        io_state.setBit( i, ( ( ( value >> ( i ) ) & 0x01 ) == 0x01 ) );
    }
#endif
    value = io_32out;
    k++;
    for ( int i = mode * k; i < mode * ( k + 1 ); i++ ) {
        io_state.setBit( i, ( ( ( value >> i ) & 0x01 ) == 0x01 ) );
    }
    k++;
    for ( int i = mode * k; i < mode * ( k + 1 ); i++ ) {
        io_state.setBit( i, ( ( ( value >> ( i ) ) & 0x01 ) == 0x01 ) );
    }

    emit sigSendIoState( io_state );
}

void IO::sendIoType() {
    emit sigSendIoType( io_type_ );
}

uint IO::getIointputA() {
    return io_32in;
}
uint IO::getIointputB() {
    return io_32in_2;
}

uint IO::getIoOutput() {
    return io_32out;
}

/**初始化读取io模块数据
 * @brief IO::initData
 */
void IO::initData() {
    //初始化IO 存入到集合
    sendDatas = new QList< QByteArray >();
    sendDatas->append( getIODataByAddress( IO_INPUT_1 ) );  //获取查询IO输入模块1数据
#ifdef INPUT_TWO
    sendDatas->append( getIODataByAddress( IO_INPUT_2 ) );
#endif
    sendDatas->append( getIODataByAddress( IO_OUTPUT_1 ) );  //获取查询IO输出模块1数据
}

/**
 * 根据IO模块地址，获取读取IO模块数据，发送的读取信息
 *
 * @brief IO::getIODataByAddress
 * @param address  IO模块地址
 * @return  发送的读取信息
 */
QByteArray IO::getIODataByAddress( IO_ADDRESS address ) {
    QByteArray result;

    if ( address == IO_INPUT_1 ) {
        //读IO控制器离散输入寄存器:01 02 00 00 00 20 79 D2
        // src：01 02 00 00 00 20
        QByteArray src( 6, 0x00 );
        src[ 0 ] = address;  //  模块地址
        src[ 1 ] = 0x02;     //功能码 读离散输入寄存器
        src[ 5 ] = 0x20;     //寄存器数量
        //追加crc信息
        QByteArray crc = Utility::CRC16_ModBus( src, src.count() );
        //查询数据:src+crc
        result.append( src ).append( crc );
    }
#ifdef INPUT_TWO
    else if ( address == IO_INPUT_2 )  //读IO控制器离散输入寄存器:01 02 00 00 00 08 79 CC
    {
#ifdef INPUT_8
        // src：01 02 00 00 00 20
        QByteArray src( 6, 0x00 );
        src[ 0 ] = address;  //  模块地址
        src[ 1 ] = 0x02;     //功能码 读离散输入寄存器
        src[ 5 ] = 0x08;     //寄存器数量
        //追加crc信息
        QByteArray crc = Utility::CRC16_ModBus( src, src.count() );
        //查询数据:src+crc
        result.append( src ).append( crc );
#endif
#ifdef INPUT_32
        QByteArray src( 6, 0x00 );
        src[ 0 ] = address;  //  模块地址
        src[ 1 ] = 0x02;     //功能码 读离散输入寄存器
        src[ 5 ] = 0x20;     //寄存器数量
        //追加crc信息
        QByteArray crc = Utility::CRC16_ModBus( src, src.count() );
        //查询数据:src+crc
        result.append( src ).append( crc );
#endif
    }
#endif
    else if ( address == IO_OUTPUT_1 )  //输出
    {
        QByteArray src( 11, 0x00 );

        src[ 0 ]  = address;
        src[ 1 ]  = 0x0f;
        src[ 5 ]  = 0x20;
        src[ 6 ]  = 0x04;
        src[ 7 ]  = static_cast< char >( io_32out );  // 1-8
        src[ 8 ]  = static_cast< char >( io_32out >> 8 );
        src[ 9 ]  = static_cast< char >( io_32out >> 16 );
        src[ 10 ] = static_cast< char >( io_32out >> 24 );  // 25-32
        //追加crc信息
        QByteArray crc = Utility::CRC16_ModBus( src, src.count() );
        //查询数据:src+crc
        result.append( src ).append( crc );
    }
    return result;
}

void IO::ioControlSetIoState() {
    if ( io_control_com->isOpened() ) {
        int replaceIndex = sendDatas->size() - 1;  //集合对应的输出模块地址,一般该数组最后一组就是输出模块
        sendDatas->replace( replaceIndex, getIODataByAddress( IO_OUTPUT_1 ) );
        emit sigSendData( io_control_com, getIODataByAddress( IO_OUTPUT_1 ) );
    }
}

void IO::ioSendData() {
    //预防空值
    if ( sendDatas->isEmpty() ) {
        initData();     //重置数据
        sendIndex = 0;  //置零
    }

    if ( is_allow_setting ) {
        //修改sendDatas集合里面的内容
        int replaceIndex = sendDatas->size() - 1;                               //集合对应的输出模块地址
        sendDatas->replace( replaceIndex, getIODataByAddress( IO_OUTPUT_1 ) );  //重新获取查询IO输出模块1数据
    }

    //串口已经打开
    if ( io_control_com->isOpened() ) {
        //下标-1，即空指针数组，返回的下标
        //定时发送内容
        if ( ( sendIndex != -1 ) && ( sendIndex < sendDatas->count() ) ) {
            emit sigSendData( io_control_com, sendDatas->at( sendIndex ) );
            sendIndex++;
            if ( sendIndex == sendDatas->count() ) {
                //下标溢出，重置下标
                sendIndex = 0;
            }
            return;
        }
    }
}

void IO::sltIOInit() {
    // qDebug() << "io_currentThreadId()==" << QThread::currentThreadId();
    //初始化数据
    initData();
    //初始化，打开串口
    io_control_com = new SerialPortObj( tr( "IO控制器" ), Param::IoControl_port_name, Param::IoControl_baud_rate );
    connect( io_control_com, SIGNAL( sigReadData( QByteArray ) ), this, SLOT( sltIORecvieData( QByteArray ) ) );
    connect( this, SIGNAL( sigSendData( SerialPortObj*, QByteArray ) ), io_control_com, SLOT( sltSendData( SerialPortObj*, QByteArray ) ) );
    io_control_com->init( false );
    //定时器，发送数据
    if ( io_control_com->isOpened() ) {
        QTimer* timer50ms = new QTimer();
        connect( timer50ms, SIGNAL( timeout() ), this, SLOT( slt50msTimer() ) );
        timer50ms->start( 50 );  //使用数据线调试需要写50ms因为要转USB，后面用50好像也是可以的，因为原点信号由驱动器自身识别
    }
    else {
        emit sigSendTips( "错误，IO模块串口打开失败" );
    }
}

void IO::slt50msTimer() {
    //发送IO状态到UI中显示
    readIoState();
    //掉线时间
    if ( output_1_lose_connect_time++ > 60 ) {
        output_1_lose_connect_time = 0;
        //发送掉线信号
        emit sigLoseConnect( "IO输出1模块" );
    }
    if ( input_1_lose_connect_time++ > 60 ) {
        input_1_lose_connect_time = 0;
        //发送掉线信号
        emit sigLoseConnect( "IO输入1模块" );
    }
#ifdef INPUT_TWO
    if ( input_2_lose_connect_time++ > 60 ) {
        input_2_lose_connect_time = 0;
        //发送掉线信号
        emit sigLoseConnect( "IO输入2模块" );
    }
#endif
    //发送IO数据
    ioSendData();
}

void IO::sltIORecvieData( const QByteArray& data ) {
    emit sigIOReceiveData( data, "IO:" );

    //输出模块的应答
    if ( data.size() == 8 ) {
        //判断输出模块的地址和功能码
        if ( ( data.at( 0 ) == IO_OUTPUT_1 ) && ( data.at( 1 ) == 0x0f ) ) {
            output_1_lose_connect_time = 0;  //掉线时间置位
        }
    }
    //输入模块的应答
    else if ( data.size() == 9 ) {
        //功能码验证
        if ( ( data.at( 1 ) != 0x02 ) || ( data.at( 2 ) != 0x04 ) ) {
            qCritical() << "输入IO功能码/数据长度异常!";
            return;
        }

        QByteArray crc( 2, 0x00 );
        crc = Utility::CRC16_ModBus( data, data.size() - 2 );

        // CRC验证
        if ( ( data.at( data.size() - 2 ) != crc[ 0 ] ) || ( data.at( data.size() - 1 ) != crc[ 1 ] ) ) {
            qCritical() << "输入IOCRC验证失败!";
            return;
        }

        char address = data.at( 0 );  //模块地址

        switch ( address ) {
        case IO_INPUT_1:
            //输入模块1
            io_32in = static_cast< uint >( ( static_cast< uchar >( data.at( 6 ) ) << 24 ) + ( static_cast< uchar >( data.at( 5 ) ) << 16 ) + ( static_cast< uchar >( data.at( 4 ) ) << 8 )
                                           + static_cast< uchar >( data.at( 3 ) ) );
            input_1_lose_connect_time = 0;

            break;

#ifdef INPUT_TWO
#ifdef INPUT_32
        case IO_INPUT_2:
            //输入模块1
            io_32in_2 = static_cast< uint >( ( static_cast< uchar >( data.at( 6 ) ) << 24 ) + ( static_cast< uchar >( data.at( 5 ) ) << 16 ) + ( static_cast< uchar >( data.at( 4 ) ) << 8 )
                                             + static_cast< uchar >( data.at( 3 ) ) );
            input_2_lose_connect_time = 0;

            break;
#endif
#endif

        default:
            break;
        }
    }
#ifdef INPUT_TWO
#ifdef INPUT_8
    // 8位输入模块
    else if ( data.size() == 6 ) {
        //功能码验证
        if ( ( data.at( 1 ) != 0x02 ) || ( data.at( 2 ) != 0x01 ) ) {
            qCritical() << "输入IO（8位）功能码/数据长度异常!";
            return;
        }

        QByteArray crc( 2, 0x00 );
        crc = Utility::CRC16_ModBus( data, data.size() - 2 );

        // CRC验证
        if ( ( data.at( data.size() - 2 ) != crc[ 0 ] ) || ( data.at( data.size() - 1 ) != crc[ 1 ] ) ) {
            qCritical() << "输入IOCRC验证失败!";
            return;
        }

        char address = data.at( 0 );  //模块地址

        switch ( address ) {
        case IO_INPUT_2:
            //输入模块1
            io_32in_2                 = static_cast< uint >( data.at( 3 ) );
            input_2_lose_connect_time = 0;

            break;

        default:
            break;
        }
    }
#endif
#endif
    else {
        qCritical() << "IO数据长度异常";
        return;
    }
}

/**
 * @brief Agv::sltToggleDoBit io_module state的点击事件触发函数.
 * @param moduleId 模块id  /0：代表0-31为输入io_ / 1：代表32-63为输出io_
 * @param id io_口id,当前为[1-64]
 */
void IO::sltToggleDoBit( int moduleId, int id ) {
    Q_UNUSED( id )
    switch ( moduleId ) {
    case 0:  //输入模块无需执行
        break;

    case 1:  //输入模块无需执行
#ifdef INPUT_ONE
        toggleDO( id - 32 );
#endif
        break;

    case 2:
#ifdef INPUT_TWO
        toggleDO( id - 64 );
#endif
        break;

    default:
        break;
    }
}
